#include "Control_Task.h"
#include "INS_Task.h"
#include "remote_control.h"
#include "ramp.h"
#include "pid.h"
#include "motor.h"
#include "arm_math.h"
#include "bsp_can.h"
#include "CAN_task.h"
#include "cmsis_os.h"
#include "Config.h"
#include "arm_math.h"
#include "math.h"
#include "lpf.h"
#include "HIPNUC.h"

Control_Info_typedef 	Control_Info ={

  .L_Leg_Info ={
	   .VMC = {
			   .L5 = 0.15f,
		     .L1 = 0.15f,
		     .L4 = 0.15f,
			   .L2 = 0.27f,
		     .L3 = 0.27f,
			   .Target_L0 = 0.20f,
		 },
	},
   .R_Leg_Info ={
	   .VMC = {
			   .L5 = 0.15f,
		     .L1 = 0.15f,
		     .L4 = 0.15f,
			   .L2 = 0.27f,
		     .L3 = 0.27f,
			 
			   .Target_L0 = 0.20f,
       
		 },
	},




};



PID_Info_TypeDef PID_Yaw[2];
PID_Info_TypeDef PID_Leg_Coordinate;
PID_Info_TypeDef PID_Leg_length[2];


static float PID_Yaw_param[2][6] = {

	[0] = {20.f,0.f,0.f,0.f,0.f,10.f},
	[1] = {3.f,0.1f,0.f,0.f,1.f,6.f},
	
};
static float PID_Leg_Coordinate_param[6]={40.f,0.f,5.0f,0.f,0.f,10};
static float PID_Leg_length_param[6]={4000.f,0.f,100000.f,0,0,100};




static void Control_Init();
static void Control_LQR_K_Update();
static void Control_Measure_Update();
static void Control_Target_Update();
static void Control_LQR_Stand_T_Balance_Tp_Calculate();
static void VMC_F_Tp_To_Joint_Calculate();
static void VMC_Calculate();
//static void Control_Jump_Calculate();

//float a11[6] = {0,-102.193167f,150.761606f,-193.615718f,0.995319f};
//float a12[6] = {0,25.734535f,-40.185849f,-24.806052f,0.259442f};
//float a13[6] = {0,-282.970172f,269.589400f,-87.682520f,-11.526496f};
//float a14[6] = {0,-170.870076f,170.442799f,-70.132389f,-9.500878f};
//float a15[6] = {0,-934.553221f,996.466873f,-393.491701f,75.307928f};
//float a16[6] = {0,-9.640385f,17.240583f,-11.328444f,5.572124f};
//float a21[6] = {0,116.456906f,-90.422065f,18.029163f,15.326962f};
//float a22[6] = {0,14.651723f,-13.314330f,7.008978f,2.069910f};
//float a23[6] = {0,-448.901498f,497.434136f,-204.856427f,38.075758f};
//float a24[6] = {0,-366.557136f,394.148167f,-156.269206f,29.028684f};
//float a25[6] = {0,1844.687048f,-1774.544648f,586.448345f,67.073989f};
//float a26[6] = {0,112.390042f,-116.315427f,43.290253f,-1.187838f};


float a11[6] = {0,-182.402741f,215.643746f,-183.199994f,0.525760f};
float a12[6] = {0,1.742048f,-11.387452f,-22.788109f,0.149689f};
float a13[6] = {0,-300.793132f,278.805940f,-86.840995f,-12.159510f};
float a14[6] = {0,-186.070427f,176.385542f,-65.828994f,-9.768206f};
float a15[6] = {0,-992.023369f,1006.238340f,-364.861844f,59.178356f};
float a16[6] = {0,-9.561895f,11.434145f,-4.930149f,2.459520f};
float a21[6] = {0,86.075995f,-30.979494f,-19.834367f,16.745139f};
float a22[6] = {0,14.223370f,-7.395980f,-0.640931f,2.353690f};
float a23[6] = {0,-478.132955f,526.986202f,-210.510569f,35.177889f};
float a24[6] = {0,-398.720407f,430.013379f,-167.507002f,27.601697f};
float a25[6] = {0,2065.050859f,-1941.463894f,618.637458f,59.956861f};
float a26[6] = {0,60.132043f,-60.354668f,21.418489f,0.226397f};


void Control_Task(void const * argument)
{
  /* USER CODE BEGIN Control_Task */
	PID_Init(&PID_Yaw[0],PID_POSITION,PID_Yaw_param[0]);
	PID_Init(&PID_Yaw[1],PID_POSITION,PID_Yaw_param[1]);
	PID_Init(&PID_Leg_Coordinate,PID_POSITION,PID_Leg_Coordinate_param);
 PID_Init(&PID_Leg_length[0],PID_POSITION,PID_Leg_length_param);
 PID_Init(&PID_Leg_length[1],PID_POSITION,PID_Leg_length_param);
	
  /* Infinite loop */
  for(;;)
  {
		Control_Init();
		  VMC_Calculate();
		Control_LQR_K_Update();
		Control_Measure_Update();
	  Control_Target_Update();
		//Control_Jump_Calculate();
	 Control_LQR_Stand_T_Balance_Tp_Calculate();
	 VMC_F_Tp_To_Joint_Calculate();
    
		
    osDelay(1);
  }
  /* USER CODE END Control_Task */
}

static void Control_Init(){
	
	Control_Info.L_Leg_Info.Target.Phi =0.0f;
	Control_Info.L_Leg_Info.Target.Theta=0.0f;
	
	Control_Info.R_Leg_Info.Target.Phi =0.0f;
	Control_Info.R_Leg_Info.Target.Theta=0.0f;
	
	Control_Info.Leg_Length_Mode = CHASSMIS_LEG_LOW;
	
	
	if(remote_ctrl.rc.s[0]==2&&remote_ctrl.rc.s[1]==1){
	  Control_Info.Control_Mode=Key_Board_Mode;
		
	}

		
		

	if(remote_ctrl.rc_lost==0){
	
		
		if(remote_ctrl.rc.s[0]==2){
			
			Control_Info.Chassmis_Mode=CHASSMIS_WEAK;
			
		}
		

		
		if(remote_ctrl.rc.s[0]==3){
			
		Control_Info.Chassmis_Mode=CHASSMIS_FLLOW;

			
		}
		if(remote_ctrl.rc.s[0]==1){
			
		Control_Info.Chassmis_Mode=CHASSMIS_SPIN;
		
			
		}
		
	
		if(remote_ctrl.rc.s[1]==2){
			
		Control_Info.Gimbal_Mode = GIMBAL_WEAK;
			
		}
		if(remote_ctrl.rc.s[1]==3){
		
			Control_Info.Gimbal_Mode = GIMBAL_ON;
			
		}
		

	}

	else if(remote_ctrl.rc_lost==1){
		
	Control_Info.Chassmis_Mode=CHASSMIS_WEAK;
  Control_Info.Gimbal_Mode = GIMBAL_WEAK;
		
	}
	
	
}
	
static void VMC_Calculate(){

	   Control_Info.L_Leg_Info.VMC.Phi1 = (3.141593f + 0.64f) + (DM_8009_Motor[0].Data.Position );
     Control_Info.L_Leg_Info.VMC.Phi4 = DM_8009_Motor[1].Data.Position - 0.64f;
  
	
	  Control_Info.R_Leg_Info.VMC.Phi1 = (3.141593f + 0.64f) + (DM_8009_Motor[3].Data.Position );
    Control_Info.R_Leg_Info.VMC.Phi4 = DM_8009_Motor[2].Data.Position - 0.64f;
	

Control_Info.L_Leg_Info.VMC.X_B =  Control_Info.L_Leg_Info.VMC.L1 * arm_cos_f32(Control_Info.L_Leg_Info.VMC.Phi1);
Control_Info.L_Leg_Info.VMC.Y_B =  Control_Info.L_Leg_Info.VMC.L1 * arm_sin_f32(Control_Info.L_Leg_Info.VMC.Phi1);
Control_Info.L_Leg_Info.VMC.X_D =  Control_Info.L_Leg_Info.VMC.L5+Control_Info.L_Leg_Info.VMC.L4 *arm_cos_f32(Control_Info.L_Leg_Info.VMC.Phi4);
Control_Info.L_Leg_Info.VMC.Y_D =  Control_Info.L_Leg_Info.VMC.L4 *arm_sin_f32(Control_Info.L_Leg_Info.VMC.Phi4);
Control_Info.L_Leg_Info.VMC.A0 = 2 * Control_Info.L_Leg_Info.VMC.L2*(Control_Info.L_Leg_Info.VMC.X_D-Control_Info.L_Leg_Info.VMC.X_B);
Control_Info.L_Leg_Info.VMC.B0 = 2 * Control_Info.L_Leg_Info.VMC.L2*(Control_Info.L_Leg_Info.VMC.Y_D-Control_Info.L_Leg_Info.VMC.Y_B);
Control_Info.L_Leg_Info.VMC.C0 = Control_Info.L_Leg_Info.VMC.L2*Control_Info.L_Leg_Info.VMC.L2 + (Control_Info.L_Leg_Info.VMC.X_D-Control_Info.L_Leg_Info.VMC.X_B)*(Control_Info.L_Leg_Info.VMC.X_D-Control_Info.L_Leg_Info.VMC.X_B)+
                                   (Control_Info.L_Leg_Info.VMC.Y_D-Control_Info.L_Leg_Info.VMC.Y_B)*(Control_Info.L_Leg_Info.VMC.Y_D-Control_Info.L_Leg_Info.VMC.Y_B)- Control_Info.L_Leg_Info.VMC.L3*Control_Info.L_Leg_Info.VMC.L3;
arm_sqrt_f32(Control_Info.L_Leg_Info.VMC.A0 *Control_Info.L_Leg_Info.VMC.A0 +Control_Info.L_Leg_Info.VMC.B0*Control_Info.L_Leg_Info.VMC.B0-Control_Info.L_Leg_Info.VMC.C0 *Control_Info.L_Leg_Info.VMC.C0 ,&Control_Info.L_Leg_Info.VMC.sqrt_A2_B2_C2);
Control_Info.L_Leg_Info.VMC.Phi2 = 2*atan2f(Control_Info.L_Leg_Info.VMC.B0+Control_Info.L_Leg_Info.VMC.sqrt_A2_B2_C2,Control_Info.L_Leg_Info.VMC.A0 +Control_Info.L_Leg_Info.VMC.C0);
Control_Info.L_Leg_Info.VMC.X_C = Control_Info.L_Leg_Info.VMC.X_B+Control_Info.L_Leg_Info.VMC.L2*arm_cos_f32(Control_Info.L_Leg_Info.VMC.Phi2);
Control_Info.L_Leg_Info.VMC.Y_C = Control_Info.L_Leg_Info.VMC.Y_B+Control_Info.L_Leg_Info.VMC.L2*arm_sin_f32(Control_Info.L_Leg_Info.VMC.Phi2);
Control_Info.L_Leg_Info.VMC.Phi3 = atan2f(Control_Info.L_Leg_Info.VMC.Y_C-Control_Info.L_Leg_Info.VMC.Y_D,Control_Info.L_Leg_Info.VMC.X_C-Control_Info.L_Leg_Info.VMC.X_D);
arm_sqrt_f32((Control_Info.L_Leg_Info.VMC.X_C-(Control_Info.L_Leg_Info.VMC.L5/2))*(Control_Info.L_Leg_Info.VMC.X_C-(Control_Info.L_Leg_Info.VMC.L5/2))+Control_Info.L_Leg_Info.VMC.Y_C *Control_Info.L_Leg_Info.VMC.Y_C,&Control_Info.L_Leg_Info.VMC.L0 );
Control_Info.L_Leg_Info.VMC.Phi0 = atan2f(Control_Info.L_Leg_Info.VMC.Y_C ,Control_Info.L_Leg_Info.VMC.X_C-(Control_Info.L_Leg_Info.VMC.L5/2));
Control_Info.L_Leg_Info.VMC.Phi0_dot = (Control_Info.L_Leg_Info.VMC.Phi0 -Control_Info.L_Leg_Info.VMC.Last_Phi0)/0.001f;
Control_Info.L_Leg_Info.VMC.Last_Phi0 = Control_Info.L_Leg_Info.VMC.Phi0 ;

Control_Info.R_Leg_Info.VMC.X_B =  Control_Info.R_Leg_Info.VMC.L1 * arm_cos_f32(Control_Info.R_Leg_Info.VMC.Phi1);
Control_Info.R_Leg_Info.VMC.Y_B =  Control_Info.R_Leg_Info.VMC.L1 * arm_sin_f32(Control_Info.R_Leg_Info.VMC.Phi1);
Control_Info.R_Leg_Info.VMC.X_D =  Control_Info.R_Leg_Info.VMC.L5+Control_Info.R_Leg_Info.VMC.L4 *arm_cos_f32(Control_Info.R_Leg_Info.VMC.Phi4);
Control_Info.R_Leg_Info.VMC.Y_D =  Control_Info.R_Leg_Info.VMC.L4 *arm_sin_f32(Control_Info.R_Leg_Info.VMC.Phi4);
Control_Info.R_Leg_Info.VMC.A0 = 2 * Control_Info.R_Leg_Info.VMC.L2*(Control_Info.R_Leg_Info.VMC.X_D-Control_Info.R_Leg_Info.VMC.X_B);
Control_Info.R_Leg_Info.VMC.B0 = 2 * Control_Info.R_Leg_Info.VMC.L2*(Control_Info.R_Leg_Info.VMC.Y_D-Control_Info.R_Leg_Info.VMC.Y_B);
Control_Info.R_Leg_Info.VMC.C0 = Control_Info.R_Leg_Info.VMC.L2*Control_Info.R_Leg_Info.VMC.L2 + (Control_Info.R_Leg_Info.VMC.X_D-Control_Info.R_Leg_Info.VMC.X_B)*(Control_Info.R_Leg_Info.VMC.X_D-Control_Info.R_Leg_Info.VMC.X_B)+
                                   (Control_Info.R_Leg_Info.VMC.Y_D-Control_Info.R_Leg_Info.VMC.Y_B)*(Control_Info.R_Leg_Info.VMC.Y_D-Control_Info.R_Leg_Info.VMC.Y_B)- Control_Info.R_Leg_Info.VMC.L3*Control_Info.R_Leg_Info.VMC.L3;
arm_sqrt_f32(Control_Info.R_Leg_Info.VMC.A0 *Control_Info.R_Leg_Info.VMC.A0 +Control_Info.R_Leg_Info.VMC.B0*Control_Info.R_Leg_Info.VMC.B0-Control_Info.R_Leg_Info.VMC.C0 *Control_Info.R_Leg_Info.VMC.C0 ,&Control_Info.R_Leg_Info.VMC.sqrt_A2_B2_C2);
Control_Info.R_Leg_Info.VMC.Phi2 = 2*atan2f(Control_Info.R_Leg_Info.VMC.B0+Control_Info.R_Leg_Info.VMC.sqrt_A2_B2_C2,Control_Info.R_Leg_Info.VMC.A0 +Control_Info.R_Leg_Info.VMC.C0);
Control_Info.R_Leg_Info.VMC.X_C = Control_Info.R_Leg_Info.VMC.X_B+Control_Info.R_Leg_Info.VMC.L2*arm_cos_f32(Control_Info.R_Leg_Info.VMC.Phi2);
Control_Info.R_Leg_Info.VMC.Y_C = Control_Info.R_Leg_Info.VMC.Y_B+Control_Info.R_Leg_Info.VMC.L2*arm_sin_f32(Control_Info.R_Leg_Info.VMC.Phi2);
Control_Info.R_Leg_Info.VMC.Phi3 = atan2f(Control_Info.R_Leg_Info.VMC.Y_C-Control_Info.R_Leg_Info.VMC.Y_D,Control_Info.R_Leg_Info.VMC.X_C-Control_Info.R_Leg_Info.VMC.X_D);
arm_sqrt_f32((Control_Info.R_Leg_Info.VMC.X_C-(Control_Info.R_Leg_Info.VMC.L5/2))*(Control_Info.R_Leg_Info.VMC.X_C-(Control_Info.R_Leg_Info.VMC.L5/2))+Control_Info.R_Leg_Info.VMC.Y_C *Control_Info.R_Leg_Info.VMC.Y_C,&Control_Info.R_Leg_Info.VMC.L0 );
Control_Info.R_Leg_Info.VMC.Phi0 = atan2f(Control_Info.R_Leg_Info.VMC.Y_C ,Control_Info.R_Leg_Info.VMC.X_C-(Control_Info.R_Leg_Info.VMC.L5/2));
Control_Info.R_Leg_Info.VMC.Phi0_dot = (Control_Info.R_Leg_Info.VMC.Phi0 -Control_Info.R_Leg_Info.VMC.Last_Phi0)/0.001f;
Control_Info.R_Leg_Info.VMC.Last_Phi0 = Control_Info.R_Leg_Info.VMC.Phi0 ;


}

void	Control_LQR_K_Update(){
	
	float L0; 
	L0= Control_Info.L_Leg_Info.VMC.L0;
	Control_Info.L_Leg_Info.LQR_K[0][0]=  a11[1]*powf(L0,3)   + a11[2]*powf(L0,2)    +a11[3]*L0    +a11[4];      
  Control_Info.L_Leg_Info.LQR_K[0][1]=  a12[1]*powf(L0,3)   + a12[2]*powf(L0,2)    +a12[3]*L0    +a12[4];
  Control_Info.L_Leg_Info.LQR_K[0][2]=  a13[1]*powf(L0,3)   + a13[2]*powf(L0,2)    +a13[3]*L0    +a13[4];
  Control_Info.L_Leg_Info.LQR_K[0][3]=  a14[1]*powf(L0,3)   + a14[2]*powf(L0,2)    +a14[3]*L0    +a14[4];
  Control_Info.L_Leg_Info.LQR_K[0][4]=  a15[1]*powf(L0,3)   + a15[2]*powf(L0,2)    +a15[3]*L0    +a15[4];
  Control_Info.L_Leg_Info.LQR_K[0][5]=  a16[1]*powf(L0,3)   + a16[2]*powf(L0,2)    +a16[3]*L0    +a16[4];

	Control_Info.L_Leg_Info.LQR_K[1][0]=   a21[1]*powf(L0,3)   + a21[2]*powf(L0,2)    +a21[3]*L0    +a21[4];     
  Control_Info.L_Leg_Info.LQR_K[0][1]=   a22[1]*powf(L0,3)   + a22[2]*powf(L0,2)    +a22[3]*L0    +a22[4];
  Control_Info.L_Leg_Info.LQR_K[0][2]=   a23[1]*powf(L0,3)   + a23[2]*powf(L0,2)    +a23[3]*L0    +a23[4];
  Control_Info.L_Leg_Info.LQR_K[0][3]=   a24[1]*powf(L0,3)   + a24[2]*powf(L0,2)    +a24[3]*L0    +a24[4];
  Control_Info.L_Leg_Info.LQR_K[1][4]=   a25[1]*powf(L0,3)   + a25[2]*powf(L0,2)    +a25[3]*L0    +a25[4];
  Control_Info.L_Leg_Info.LQR_K[1][5]=   a26[1]*powf(L0,3)   + a26[2]*powf(L0,2)    +a26[3]*L0    +a26[4];
	
	float L1;
	L1= Control_Info.R_Leg_Info.VMC.L0;
	Control_Info.R_Leg_Info.LQR_K[0][0]=  a11[1]*powf(L1,3)   + a11[2]*powf(L1,2)    +a11[3]*L1    +a11[4];      
  Control_Info.R_Leg_Info.LQR_K[0][1]=  a12[1]*powf(L1,3)   + a12[2]*powf(L1,2)    +a12[3]*L1    +a12[4];
  Control_Info.R_Leg_Info.LQR_K[0][2]=  a13[1]*powf(L1,3)   + a13[2]*powf(L1,2)    +a13[3]*L1    +a13[4];
  Control_Info.R_Leg_Info.LQR_K[0][3]=  a14[1]*powf(L1,3)   + a14[2]*powf(L1,2)    +a14[3]*L1    +a14[4];
  Control_Info.R_Leg_Info.LQR_K[0][4]=  a15[1]*powf(L1,3)   + a15[2]*powf(L1,2)    +a15[3]*L1    +a15[4];
  Control_Info.R_Leg_Info.LQR_K[0][5]=  a16[1]*powf(L1,3)   + a16[2]*powf(L1,2)    +a16[3]*L1    +a16[4];

	Control_Info.R_Leg_Info.LQR_K[1][0]=   a21[1]*powf(L1,3)   + a21[2]*powf(L1,2)    +a21[3]*L1    +a21[4];     
  Control_Info.R_Leg_Info.LQR_K[1][1]=   a22[1]*powf(L1,3)   + a22[2]*powf(L1,2)    +a22[3]*L1    +a22[4];
  Control_Info.R_Leg_Info.LQR_K[1][2]=   a23[1]*powf(L1,3)   + a23[2]*powf(L1,2)    +a23[3]*L1    +a23[4];
  Control_Info.R_Leg_Info.LQR_K[1][3]=   a24[1]*powf(L1,3)   + a24[2]*powf(L1,2)    +a24[3]*L1    +a24[4];
  Control_Info.R_Leg_Info.LQR_K[1][4]=   a25[1]*powf(L1,3)   + a25[2]*powf(L1,2)    +a25[3]*L1    +a25[4];
  Control_Info.R_Leg_Info.LQR_K[1][5]=   a26[1]*powf(L1,3)   + a26[2]*powf(L1,2)    +a26[3]*L1    +a26[4];
}



void Control_Measure_Update(){

Control_Info.L_Leg_Info.Measure.Theta = (1.5707965f - Control_Info.L_Leg_Info.VMC.Phi0) - Control_Info.L_Leg_Info.Measure.Phi ;
Control_Info.L_Leg_Info.Measure.Theta_dot = (Control_Info.L_Leg_Info.Measure.Theta -Control_Info.L_Leg_Info.Measure.Last_Theta)/0.001f;
Control_Info.L_Leg_Info.Measure.Last_Theta = Control_Info.L_Leg_Info.Measure.Theta ;
Control_Info.L_Leg_Info.Measure.Phi = -INS_Info.Angle[2];
Control_Info.L_Leg_Info.Measure.Phi_dot = -INS_Info.Gyro[0];
	
Control_Info.R_Leg_Info.Measure.Theta = (Control_Info.R_Leg_Info.VMC.Phi0 - 1.5707965f) - Control_Info.R_Leg_Info.Measure.Phi ;
Control_Info.R_Leg_Info.Measure.Theta_dot = (Control_Info.R_Leg_Info.Measure.Theta -Control_Info.R_Leg_Info.Measure.Last_Theta)/0.001f;
Control_Info.R_Leg_Info.Measure.Last_Theta = Control_Info.R_Leg_Info.Measure.Theta ;
Control_Info.R_Leg_Info.Measure.Phi = -INS_Info.Angle[2];
Control_Info.R_Leg_Info.Measure.Phi_dot = -INS_Info.Gyro[0];
		

//	Control_Info.Target_Chassis_Velocity = -remote_ctrl.rc.ch[1]*0.0001f;
//	
//	Control_Info.L_Leg_Info.w =  ( -GIM_8108_Motor[0].Data.Velocity - Control_Info.L_Leg_Info.VMC.Phi0_dot + INS_Info.Gyro[0]) *0.08f
//	                      	+ ( Control_Info.L_Leg_Info.VMC.L0 * Control_Info.L_Leg_Info.Measure.Theta_dot * arm_cos_f32(Control_Info.L_Leg_Info.Measure.Theta)) ;
//	
//		Control_Info.L_Leg_Info.Predict_Velocity = Control_Info.L_Leg_Info.Fusion_Velocity + Control_Info.Accel*0.001f;
//	
//	Control_Info.L_Leg_Info.Fusion_Velocity =  0.8f*Control_Info.L_Leg_Info.Predict_Velocity +0.2f*Control_Info.L_Leg_Info.w;
//	
//	Control_Info.L_Leg_Info.Measure.Chassis_Velocity = Control_Info.L_Leg_Info.Fusion_Velocity;
//	
//	
//	
//	Control_Info.R_Leg_Info.w = ( GIM_8108_Motor[1].Data.Velocity  +  Control_Info.R_Leg_Info.VMC.Phi0_dot + INS_Info.Gyro[0] ) *0.08f
//	                                         + ( Control_Info.R_Leg_Info.VMC.L0 * Control_Info.R_Leg_Info.Measure.Theta_dot * arm_cos_f32(Control_Info.R_Leg_Info.Measure.Theta));
//	
//	Control_Info.R_Leg_Info.Predict_Velocity = Control_Info.R_Leg_Info.Fusion_Velocity + Control_Info.Accel*0.001f;
//	
//	Control_Info.R_Leg_Info.Fusion_Velocity =  0.8f*Control_Info.R_Leg_Info.Predict_Velocity +0.2f*Control_Info.R_Leg_Info.w;
//	
//	Control_Info.R_Leg_Info.Measure.Chassis_Velocity = Control_Info.R_Leg_Info.Fusion_Velocity;	
//	
//	 Control_Info.Chassis_Velocity = (Control_Info.L_Leg_Info.Measure.Chassis_Velocity +Control_Info.R_Leg_Info.Measure.Chassis_Velocity )/2;
//	
//if(Control_Info.Target_Chassis_Velocity!=0)
//{
//	Control_Info.L_Leg_Info.Measure.Chassis_Position +=Control_Info.L_Leg_Info.Measure.Chassis_Velocity *0.001f;
//  Control_Info.R_Leg_Info.Measure.Chassis_Position +=Control_Info.R_Leg_Info.Measure.Chassis_Velocity *0.001f;

//}else{
//	

//	 Control_Info.L_Leg_Info.Measure.Chassis_Position=0;
//   Control_Info.R_Leg_Info.Measure.Chassis_Position=0;
//	
//	
//}


//Control_Info.Accel = ( -INS_Info.Accel[1] - GravityAccel* arm_sin_f32 ( -INS_Info.Angle[2]) * arm_cos_f32 (-INS_Info.Angle[2]));

		Control_Info.Target_Yaw += remote_ctrl.rc.ch[0]*0.0002f; 

	if( Control_Info.Target_Yaw >= 180.f) Control_Info.Target_Yaw -=360.f;
	if( Control_Info.Target_Yaw <= -180.f) Control_Info.Target_Yaw +=360.f;

Control_Info.Yaw_Err = Control_Info.Target_Yaw - INS_Info.Yaw_Angle;

	  if( Control_Info.Yaw_Err >= 180.f) Control_Info.Yaw_Err -=360.f;
			if( Control_Info.Yaw_Err <= -180.f) Control_Info.Yaw_Err +=360.f;

      f_PID_Calculate(&PID_Yaw[0],Control_Info.Yaw_Err*0.0174532944f,0);
		  f_PID_Calculate(&PID_Yaw[1],PID_Yaw[0].Output,INS_Info.Gyro[2]);



}



	void  Control_Target_Update(){
	Control_Info.L_Leg_Info.Target.Theta=0;
	Control_Info.L_Leg_Info.Target.Theta_dot=0;
	Control_Info.L_Leg_Info.Target.Chassis_Position=0;
	Control_Info.L_Leg_Info.Target.Phi=0;
	Control_Info.L_Leg_Info.Target.Phi_dot=0;
	
	Control_Info.R_Leg_Info.Target.Theta=0;
	Control_Info.R_Leg_Info.Target.Theta_dot=0;
	Control_Info.R_Leg_Info.Target.Chassis_Position=0;
	Control_Info.R_Leg_Info.Target.Phi=0;
	Control_Info.R_Leg_Info.Target.Phi_dot=0;
	
	
	}


//	void Control_Jump_Calculate(){
//	
//	Control_Info.L_Leg_Info.Measure.F = (((DM_8009_Motor[1].Data.Torque * arm_cos_f32(( Control_Info.L_Leg_Info.VMC.Phi0 -  Control_Info.L_Leg_Info.VMC.Phi3)))/
//                                       (0.15f *  arm_sin_f32 (( Control_Info.L_Leg_Info.VMC.Phi3 - (Control_Info.L_Leg_Info.VMC.Phi4)))))      -
//	                                    ((DM_8009_Motor[0].Data.Torque * arm_cos_f32(( Control_Info.L_Leg_Info.VMC.Phi0 -  Control_Info.L_Leg_Info.VMC.Phi2)))/
//                                       (0.15f * arm_sin_f32 ((Control_Info.L_Leg_Info.VMC.Phi1 - (Control_Info.L_Leg_Info.VMC.Phi2))))));

// Control_Info.L_Leg_Info.Measure.Tp= Control_Info.L_Leg_Info.VMC.L0*(((DM_8009_Motor[0].Data.Torque * arm_sin_f32(( Control_Info.L_Leg_Info.VMC.Phi0 -  Control_Info.L_Leg_Info.VMC.Phi2)))/
//                                       (0.15f *  arm_sin_f32 (( Control_Info.L_Leg_Info.VMC.Phi1 - (Control_Info.L_Leg_Info.VMC.Phi2)))))      -
//	                                    ((DM_8009_Motor[1].Data.Torque * arm_sin_f32(( Control_Info.L_Leg_Info.VMC.Phi0 -  Control_Info.L_Leg_Info.VMC.Phi3)))/
//                                       (0.15f * arm_sin_f32 ((Control_Info.L_Leg_Info.VMC.Phi3 - (Control_Info.L_Leg_Info.VMC.Phi4))))));

//	Control_Info.R_Leg_Info.Measure.F = (((DM_8009_Motor[2].Data.Torque * arm_cos_f32(( Control_Info.R_Leg_Info.VMC.Phi0 -  Control_Info.R_Leg_Info.VMC.Phi3)))/
//                                       (0.15f *  arm_sin_f32 (( Control_Info.R_Leg_Info.VMC.Phi3 - (Control_Info.R_Leg_Info.VMC.Phi4)))))      -
//	                                    ((DM_8009_Motor[3].Data.Torque * arm_cos_f32(( Control_Info.R_Leg_Info.VMC.Phi0 -  Control_Info.R_Leg_Info.VMC.Phi2)))/
//                                       (0.15f * arm_sin_f32 ((Control_Info.R_Leg_Info.VMC.Phi1 - (Control_Info.R_Leg_Info.VMC.Phi2))))));

// Control_Info.R_Leg_Info.Measure.Tp= Control_Info.R_Leg_Info.VMC.L0*(((DM_8009_Motor[3].Data.Torque * arm_sin_f32(( Control_Info.R_Leg_Info.VMC.Phi0 -  Control_Info.R_Leg_Info.VMC.Phi2)))/
//                                       (0.15f *  arm_sin_f32 (( Control_Info.R_Leg_Info.VMC.Phi1 - (Control_Info.R_Leg_Info.VMC.Phi2)))))      -
//	                                    ((DM_8009_Motor[2].Data.Torque * arm_sin_f32(( Control_Info.R_Leg_Info.VMC.Phi0 -  Control_Info.R_Leg_Info.VMC.Phi3)))/
//                                       (0.15f * arm_sin_f32 ((Control_Info.R_Leg_Info.VMC.Phi3 - (Control_Info.R_Leg_Info.VMC.Phi4))))));
//	
//	
//	
//	
//	
//	
//	
//	}
//	
	
void Control_LQR_Stand_T_Balance_Tp_Calculate(){

Control_Info.L_Leg_Info.LQR_X[0] =  Control_Info.L_Leg_Info.Target.Theta - Control_Info.L_Leg_Info.Measure.Theta;
Control_Info.L_Leg_Info.LQR_X[1] =  Control_Info.L_Leg_Info.Target.Theta_dot - Control_Info.L_Leg_Info.Measure.Theta_dot;
Control_Info.L_Leg_Info.LQR_X[2] = Control_Info.L_Leg_Info.Target.Chassis_Position - Control_Info.L_Leg_Info.Measure.Chassis_Position;
Control_Info.L_Leg_Info.LQR_X[3] = Control_Info.Target_Chassis_Velocity - Control_Info.Chassis_Velocity;
Control_Info.L_Leg_Info.LQR_X[4] = Control_Info.L_Leg_Info.Target.Phi - Control_Info.L_Leg_Info.Measure.Phi;
Control_Info.L_Leg_Info.LQR_X[5] = Control_Info.R_Leg_Info.Target.Phi_dot - Control_Info.L_Leg_Info.Measure.Phi_dot;	
	
Control_Info.R_Leg_Info.LQR_X[0] = Control_Info.R_Leg_Info.Target.Theta - Control_Info.R_Leg_Info.Measure.Theta;
Control_Info.R_Leg_Info.LQR_X[1] = Control_Info.R_Leg_Info.Target.Theta_dot -Control_Info.R_Leg_Info.Measure.Theta_dot;
Control_Info.R_Leg_Info.LQR_X[2] = Control_Info.R_Leg_Info.Target.Chassis_Position - Control_Info.R_Leg_Info.Measure.Chassis_Position;
Control_Info.R_Leg_Info.LQR_X[3] = Control_Info.Target_Chassis_Velocity - Control_Info.Chassis_Velocity;
Control_Info.R_Leg_Info.LQR_X[4] =Control_Info.R_Leg_Info.Target.Phi -Control_Info.R_Leg_Info.Measure.Phi;
Control_Info.R_Leg_Info.LQR_X[5] =Control_Info.R_Leg_Info.Target.Phi_dot -Control_Info.R_Leg_Info.Measure.Phi_dot;	
	
	
	Control_Info.L_Leg_Info.LQR_Output[0][0] = Control_Info.L_Leg_Info.LQR_X[0]*Control_Info.L_Leg_Info.LQR_K[0][0];
	Control_Info.L_Leg_Info.LQR_Output[0][1] = Control_Info.L_Leg_Info.LQR_X[1]*Control_Info.L_Leg_Info.LQR_K[0][1];
	Control_Info.L_Leg_Info.LQR_Output[0][2] = Control_Info.L_Leg_Info.LQR_X[2]*Control_Info.L_Leg_Info.LQR_K[0][2];
	Control_Info.L_Leg_Info.LQR_Output[0][3] = Control_Info.L_Leg_Info.LQR_X[3]*Control_Info.L_Leg_Info.LQR_K[0][3];
	Control_Info.L_Leg_Info.LQR_Output[0][4] = Control_Info.L_Leg_Info.LQR_X[4]*Control_Info.L_Leg_Info.LQR_K[0][4];
	Control_Info.L_Leg_Info.LQR_Output[0][5] = Control_Info.L_Leg_Info.LQR_X[5]*Control_Info.L_Leg_Info.LQR_K[0][5];
	
	Control_Info.L_Leg_Info.Moment.T = -( Control_Info.L_Leg_Info.LQR_Output[0][0]+Control_Info.L_Leg_Info.LQR_Output[0][1]+Control_Info.L_Leg_Info.LQR_Output[0][2]
	                                  +Control_Info.L_Leg_Info.LQR_Output[0][3]+Control_Info.L_Leg_Info.LQR_Output[0][4]+Control_Info.L_Leg_Info.LQR_Output[0][5]);
	
	VAL_LIMIT(Control_Info.L_Leg_Info.Moment.T,-6.f,6.f);	
	
	Control_Info.L_Leg_Info.LQR_Output[1][0] = Control_Info.L_Leg_Info.LQR_X[0]*Control_Info.L_Leg_Info.LQR_K[1][0];
	Control_Info.L_Leg_Info.LQR_Output[1][1] = Control_Info.L_Leg_Info.LQR_X[1]*Control_Info.L_Leg_Info.LQR_K[1][1];
	Control_Info.L_Leg_Info.LQR_Output[1][2] = Control_Info.L_Leg_Info.LQR_X[2]*Control_Info.L_Leg_Info.LQR_K[1][2];
	Control_Info.L_Leg_Info.LQR_Output[1][3] = Control_Info.L_Leg_Info.LQR_X[3]*Control_Info.L_Leg_Info.LQR_K[1][3];
	Control_Info.L_Leg_Info.LQR_Output[1][4] = Control_Info.L_Leg_Info.LQR_X[4]*Control_Info.L_Leg_Info.LQR_K[1][4];
	Control_Info.L_Leg_Info.LQR_Output[1][5] = Control_Info.L_Leg_Info.LQR_X[5]*Control_Info.L_Leg_Info.LQR_K[1][5];
	
	Control_Info.L_Leg_Info.Moment.Tp = ( Control_Info.L_Leg_Info.LQR_Output[1][0]+Control_Info.L_Leg_Info.LQR_Output[1][1]+Control_Info.L_Leg_Info.LQR_Output[1][2]
	                                  +Control_Info.L_Leg_Info.LQR_Output[1][3]+Control_Info.L_Leg_Info.LQR_Output[1][4]+Control_Info.L_Leg_Info.LQR_Output[1][5]);
	
	VAL_LIMIT( Control_Info.L_Leg_Info.Moment.Tp,-40.f,40.f);	
	
	Control_Info.R_Leg_Info.LQR_Output[0][0] = Control_Info.R_Leg_Info.LQR_X[0]*Control_Info.R_Leg_Info.LQR_K[0][0];
	Control_Info.R_Leg_Info.LQR_Output[0][1] = Control_Info.R_Leg_Info.LQR_X[1]*Control_Info.R_Leg_Info.LQR_K[0][1];
	Control_Info.R_Leg_Info.LQR_Output[0][2] = Control_Info.R_Leg_Info.LQR_X[2]*Control_Info.R_Leg_Info.LQR_K[0][2];
	Control_Info.R_Leg_Info.LQR_Output[0][3] = Control_Info.R_Leg_Info.LQR_X[3]*Control_Info.R_Leg_Info.LQR_K[0][3];
	Control_Info.R_Leg_Info.LQR_Output[0][4] = Control_Info.R_Leg_Info.LQR_X[4]*Control_Info.R_Leg_Info.LQR_K[0][4];
	Control_Info.R_Leg_Info.LQR_Output[0][5] = Control_Info.R_Leg_Info.LQR_X[5]*Control_Info.R_Leg_Info.LQR_K[0][5];
	
	Control_Info.R_Leg_Info.Moment.T =Control_Info.R_Leg_Info.LQR_Output[0][0]+Control_Info.R_Leg_Info.LQR_Output[0][1]+Control_Info.R_Leg_Info.LQR_Output[0][2]
	                                  +Control_Info.R_Leg_Info.LQR_Output[0][3]+Control_Info.R_Leg_Info.LQR_Output[0][4]+Control_Info.R_Leg_Info.LQR_Output[0][5];
	
	VAL_LIMIT(Control_Info.R_Leg_Info.Moment.T,-6.f,6.f);	
	
	Control_Info.R_Leg_Info.LQR_Output[1][0] = Control_Info.R_Leg_Info.LQR_X[0]*Control_Info.R_Leg_Info.LQR_K[1][0];
	Control_Info.R_Leg_Info.LQR_Output[1][1] = Control_Info.R_Leg_Info.LQR_X[1]*Control_Info.R_Leg_Info.LQR_K[1][1];
	Control_Info.R_Leg_Info.LQR_Output[1][2] = Control_Info.R_Leg_Info.LQR_X[2]*Control_Info.R_Leg_Info.LQR_K[1][2];
	Control_Info.R_Leg_Info.LQR_Output[1][3] = Control_Info.R_Leg_Info.LQR_X[3]*Control_Info.R_Leg_Info.LQR_K[1][3];
	Control_Info.R_Leg_Info.LQR_Output[1][4] = Control_Info.R_Leg_Info.LQR_X[4]*Control_Info.R_Leg_Info.LQR_K[1][4];
	Control_Info.R_Leg_Info.LQR_Output[1][5] = Control_Info.R_Leg_Info.LQR_X[5]*Control_Info.R_Leg_Info.LQR_K[1][5];
	
	Control_Info.R_Leg_Info.Moment.Tp = Control_Info.R_Leg_Info.LQR_Output[1][0]+Control_Info.R_Leg_Info.LQR_Output[1][1]+Control_Info.R_Leg_Info.LQR_Output[1][2]
	                                  +Control_Info.R_Leg_Info.LQR_Output[1][3]+Control_Info.R_Leg_Info.LQR_Output[1][4]+Control_Info.R_Leg_Info.LQR_Output[1][5];
	
	VAL_LIMIT( Control_Info.R_Leg_Info.Moment.Tp,-40.f,40.f);	



Control_Info.L_Leg_Info.Moment.T = Control_Info.L_Leg_Info.Moment.T + PID_Yaw[1].Output;
Control_Info.R_Leg_Info.Moment.T = Control_Info.R_Leg_Info.Moment.T - PID_Yaw[1].Output;




}


void 	 VMC_F_Tp_To_Joint_Calculate(){
	
  Control_Info.L_Leg_Info.Moment.F = 10+f_PID_Calculate(&PID_Leg_length[0],Control_Info.L_Leg_Info.VMC.Target_L0,Control_Info.L_Leg_Info.VMC.L0);
	Control_Info.R_Leg_Info.Moment.F = 10+f_PID_Calculate(&PID_Leg_length[1],Control_Info.R_Leg_Info.VMC.Target_L0,Control_Info.R_Leg_Info.VMC.L0);
	
	Control_Info.L_Leg_Info.Phi0_3 = Control_Info.L_Leg_Info.VMC.Phi0 - Control_Info.L_Leg_Info.VMC.Phi3; 
	Control_Info.L_Leg_Info.Phi1_2 = Control_Info.L_Leg_Info.VMC.Phi1 - Control_Info.L_Leg_Info.VMC.Phi2; 
	Control_Info.L_Leg_Info.Phi3_2 = Control_Info.L_Leg_Info.VMC.Phi3 - Control_Info.L_Leg_Info.VMC.Phi2; 
	Control_Info.L_Leg_Info.Phi0_2 = Control_Info.L_Leg_Info.VMC.Phi0 - Control_Info.L_Leg_Info.VMC.Phi2; 
	Control_Info.L_Leg_Info.Phi3_4 = Control_Info.L_Leg_Info.VMC.Phi3 - Control_Info.L_Leg_Info.VMC.Phi4; 
	
	Control_Info.L_Leg_Info.SendValue.T1 = Control_Info.L_Leg_Info.VMC.L1*arm_sin_f32(Control_Info.L_Leg_Info.Phi1_2)*
	                                       (Control_Info.L_Leg_Info.Moment.F*Control_Info.L_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.L_Leg_Info.Phi0_3)+Control_Info.L_Leg_Info.Moment.Tp*arm_cos_f32(Control_Info.L_Leg_Info.Phi0_3))
	                                         /Control_Info.L_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.L_Leg_Info.Phi3_2);

 Control_Info.L_Leg_Info.SendValue.T2 = Control_Info.L_Leg_Info.VMC.L4*arm_sin_f32(Control_Info.L_Leg_Info.Phi3_4)*
	                                       (Control_Info.L_Leg_Info.Moment.F*Control_Info.L_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.L_Leg_Info.Phi0_2)+Control_Info.L_Leg_Info.Moment.Tp*arm_cos_f32(Control_Info.L_Leg_Info.Phi0_2))
	                                           /Control_Info.L_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.L_Leg_Info.Phi3_2);


	Control_Info.R_Leg_Info.Phi0_3 = Control_Info.R_Leg_Info.VMC.Phi0 - Control_Info.R_Leg_Info.VMC.Phi3; 
	Control_Info.R_Leg_Info.Phi1_2 = Control_Info.R_Leg_Info.VMC.Phi1 - Control_Info.R_Leg_Info.VMC.Phi2; 
	Control_Info.R_Leg_Info.Phi3_2 = Control_Info.R_Leg_Info.VMC.Phi3 - Control_Info.R_Leg_Info.VMC.Phi2; 
	Control_Info.R_Leg_Info.Phi0_2 = Control_Info.R_Leg_Info.VMC.Phi0 - Control_Info.R_Leg_Info.VMC.Phi2; 
	Control_Info.R_Leg_Info.Phi3_4 = Control_Info.R_Leg_Info.VMC.Phi3 - Control_Info.R_Leg_Info.VMC.Phi4; 
	
	Control_Info.R_Leg_Info.SendValue.T1 = Control_Info.R_Leg_Info.VMC.L1*arm_sin_f32(Control_Info.R_Leg_Info.Phi1_2)*
	                                       (Control_Info.R_Leg_Info.Moment.F*Control_Info.R_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.R_Leg_Info.Phi0_3)+Control_Info.R_Leg_Info.Moment.Tp*arm_cos_f32(Control_Info.R_Leg_Info.Phi0_3))
	                                         /Control_Info.R_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.R_Leg_Info.Phi3_2);

 Control_Info.R_Leg_Info.SendValue.T2 = Control_Info.R_Leg_Info.VMC.L4*arm_sin_f32(Control_Info.R_Leg_Info.Phi3_4)*
	                                       (Control_Info.R_Leg_Info.Moment.F*Control_Info.R_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.R_Leg_Info.Phi0_2)+Control_Info.R_Leg_Info.Moment.Tp*arm_cos_f32(Control_Info.R_Leg_Info.Phi0_2))
	                                           /Control_Info.R_Leg_Info.VMC.L0*arm_sin_f32(Control_Info.R_Leg_Info.Phi3_2);



	
}



